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I.  INTRODUCTION 

Accurate  path  keeping  of  a  marine  vehicle  through 
prescribed  routes  in  space  is  one  of  the  most  important 
functions  of  both  manned  and  unmanned  vehicles  [Ref.  1].  This 
can  be  achieved  by  a  number  of  ways,  two  of  the  most  popular 
ones  being  line  of  sight  [Ref.  2]  and  cross  track  error 
guidance  [Ref.  3].  In  general,  cross  track  error  schemes  are 
superior  provided  an  accurate  representation  of  vehicle 
dynamics  is  available.  Cross  track  error  control  is  very 
efficient  for  straight  line  reference  paths.  Switching 
between  consecutive  straight  line  segments  is  inaccurate  since 
no  reference  path  is  explicitly  accounted  for  [Ref.  3].  This 
problem  is  addressed  in  this  work  where  we  concentrate  on  two 
different  aspects  of  the  general  path  keeping  problem.  The 
first  aspect  is  the  generation  of  a  smooth  reference  path 
which  joins  two  consecutive  straight  line  segments. 

In  ship  dynamics  problems  it  is  necessary  to  introduce  two 
different  coordinate  systems  [Ref.  4].  A  global  coordinate 
system  is  used  to  monitor  the  absolute  location  of  the  marine 
vehicle  with  respect  to  a  fixed  reference  point,  while  a  local 
(x,y)  frame  is  introduced  to  express  the  relative  location  of 
the  vehicle  with  respect  to  the  desired  steady  state  path. 
This  coincides  with  the  local  x-axis,  therefore  the  local 
frame  is  recomputed  each  time  a  new  straight  line  segment  is 


commanded.  We  then  seek  a  smooth  reference  path  that 
initiates  at  the  current  position  of  the  vehicle  and 
terminates  at  the  desired  straight  line  segment,  the  x-axis. 
If  we  nondemensionalize  x  by  the  vehicles  length  L,  time  t  by 
L,  and  the  constant  vehicle  forward  speed  u,  then  x  and  t 
assume  identical  numerical  values  at  steady  state.  Therefore, 
for  small  deviations  in  y,  a  (t,y)  graph  can  be  interpreted  as 
(x,y)  as  well.  This  allows  us  to  use  an  ordinary  differential 
equation  to  model  the  reference  path  in  the  local  coordinate 
frame. 

After  a  smooth  reference  path  has  been  generated,  we 
proceed  with  the  control  design;  we  seek  a  rudder  control  law 
which  will  drive  and  keep  the  vehicle  on  the  reference  path. 
This  is  accomplished  by  using  the  Linear  Quadratic  Regulator 
design  [Ref.  5].  A  performance  index  which  penalizes  the 
deviation  of  the  actual  path  from  the  reference  path  is 
formulated  and  minimized.  Finally,  we  present  a  series  of 
numerical  simulations  which  support  the  techniques  used  in 
this  work,  along  with  some  recommendations  for  further 
research. 


II.  REFERENCE  PATH  DEVELOPMENT 

A.  INTRODUCTION 

We  break  the  problem  of  generating  a  smooth  reference  path 
for  the  vehicle  to  follow  into  two  parts.  In  the  first  part, 
we  examine  the  lane  changing  maneuver;  i.e.,  the  change  from 
one  straight  line  path  to  a  parallel  path.  Then  we  address 
the  more  general  case  of  path  following  where  the  two  straight 
line  paths,  initial  and  final  form  an  arbitrary  angle  on  the 
plane. 

B.  LANE  CHANGING 

Figure  2.1  represents  the  type  of  path  that  we  are 
interested  in  during  a  lane  changing  maneuver.  This  type  of 
path  can  be  achieved  when  using  a  differential  equation  whose 
solution  is  exponential.  A  first  order  equation  would 
generate  a  path  that  is  too  steep  for  the  vehicle  to  follow, 
while  on  the  other  hand,  a  third  or  higher  order  equation 
would  result  in  oscillatory  response  with  multiple  overshoots 
of  the  desired  steady  state  path.  For  the  purposes  of  this 
thesis  it  was  felt  that  a  second  order  system  would  provide  a 
good  compromise  between  fast  response  and  smooth  changes. 
By  using  the  second  order  differential  equation 


initial  path 


reference  lane 
changing  path 


final  path 


Figure  2.1  Desired  Reference  Path 


y+2Ccony+coJy=0  (2.1) 

with  C  representing  the  damping  in  the  system  and  (^ 
representing  the  speed  of  the  response  of  the  system,  the 
following  types  of  paths  can  be  produced  using  different  £  and 
(4,.  (Figures  2.2  and  2.3)  The  larger  the  value  of  co^  the  quicker 
the  system  response  and  the  smaller  the  £  the  more  the  system 
over  shoots  the  path. 

After  review  of  the  above  figures,  a  damping  ratio,  £  =  1, 
was  chosen.  This  seems  to  provide  the  smoothest  path  possible 
with  no  overshoot  of  the  path.  A  natural  frequency,  oo^  =  1, 
was  chosen  to  provide  a  reasonable  time  response  of  the 
system.  By  choosing  (^  =  1  we  have  accounted  for  the  fact 
that  a  physical  system  with  certain  mechanical  restraints  is 
being  represented  and  thus  we  allow  for  the  system  to  respond 
as  quickly  as  possible  without  overworking. 

Solving  the  above  differential  equation,  the  following 
equations  are  formed: 

y(t)=y0e-c(l  +  t)  (2.2) 

y(t)=-yQte-t  (2.3) 

Plotting  eqn  (2.2)  the  desired  reference  path  shown  in  figure 
2.4  was  produced. 
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Figure  2.2  Different  Reference  Paths;  Fixed  £  and  Varying  c^ 
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Figure  2.3  Different  Reference  Paths;  Fixed  o^  and  Varying  C 


E 


Figure  2.4  Desired  Reference  Path 


C.   GENERAL  REFERENCE  PATHS 

In  the  development  of  the  reference  path  the  angle  between 
the  two  paths  was  assumed  to  be  zero.  This  angle  is 
designated  as  a.  By  using  different  angles  between  paths,  it 
was  found  that  the  smooth  path  developed  for  the  zero  angle 
was  soon  lost.  As  shown  in  figure  2.5,  excessive  path 
overshoot  may  develop  as  the  angle  a  approaches  90°.  Although 
the  system  remains  critically  damped,  the  path  overshoot  is 
non-zero  due  to  the  non-zero  initial  velocities  which  is 
increased  as  a  ■*  90°. 

It  appeared,  therefore,  that  the  reference  path  was  only 
good  for  a  zero  angle  between  the  two  paths.  However,  it  was 
felt  that  if  we  utilized  only  that  portion  of  the  path  that 
corresponded  to  the  desired  angle,  a  smooth  reference  path 
with  zero  overshoot  could  still  be  produced. 

This  is  accomplished  by  breaking  the  reference  path  into 
two  parts.  The  first  is  the  time  that  it  takes  for  the  path 
to  get  to  the  desired  angle  a.  This  is  denoted  as  T  shown  in 
figure  2.6.  The  second  is  the  time  to  finish  the  reference 
path  from  the  angle  a.  This  is  denoted  as  t.  The  time  t  is 
the  start  point  of  the  portion  of  the  reference  path  that  is 
needed.  In  actuality  the  entire  reference  path  is  produced 
but  only  that  portion  of  the  path  that  is  needed  is  used  in 
the  simulation  and  control  program. 


If  we  assume  dy(0)/dt  =  v0  and  y(0)  =  a  we  obtain  the 
following  from  eqns  (2.2)  and  (2.3), 

v0  =  -y0te-c  (2.4) 

a=y0e"t(l  +  t)  (2.5) 

with  a  representing  the  y  axis  position  and  v0  representing 
the  initial  angle  from  the  horizontal  of  the  path  which  is  the 
same  as  a.  If  we  divide  by  v0  by  a  we  obtain  the  time  it 
takes  to  get  to  angle  a. 

%.-'  (2.6) 

a   1+t 

This  is  T.  Using  T  we  put  this  back  into  our  original 
equation  and  we  get, 

y(t)=y0exp"(t+:r)  (1  +  t+T)  (2.7) 

y(t)=-(t  +  :r)y0exp-<t+r>.  (2.8) 

These  equations  provide  the  basis  for  the  desired  smooth 
reference  path.  A  family  of  reference  paths  produced  using 
this  technique  is  shown  in  figure  2.7  for  different  initial 
slopes  a.  It  can  be  seen  that  incorporating  the  above  time  T, 
the  reference  path  maintains  the  general  shape  shown  in  figure 
2.4  regardless  of  the  angle  a. 
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Figure  2.5  Path  Overshoot  for  Varying  Angles 
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Figure  2.6  Reference  Path  Configuration 
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Figure  2.7  Family  of  Desired  Reference  Paths 
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III.  EQUATIONS  OF  MOTION  AND  LQR  MINIMIZATION  DEVELOPMENT 

A.  INTRODUCTION 

Now  that  a  reference  path  has  been  developed,  an  overview 
of  the  appropriate  equations  of  motion  and  minimization  needs 
to  be  presented.  We  are  only  concerned  with  the  horizontal 
plane,  which  means  that  only  the  steering  aspect  of  the  test 
vehicle  will  be  considered.  This  chapter  will  examine  what 
are  the  equations  of  motions,  their  augmentation  with  the 
reference  path  equations,  and  the  LQR  minimization  used  in 
order  to  obtain  controller  gains. 

B.  EQUATIONS  OF  MOTION 

By  using  a  nondimentionalized  linearized  set  of  equations 
of  motion  for  the  horizontal  plane,  the  following  equations 
are  developed  and  shown  in  state  space  form. 

ijj=r  (3.1a) 

v=alxv+a12r+bxb  (3.1b) 

±=a21v+a22r+b2&  (3.1c) 

y=vcos  (\|r)  +sin(i|r)  (3. Id) 

With   an   =   -1.526394,    a12    =    -0.5096502,    a21   =    0.028750753, 
a22   =   -1.449505,    bx   =   0.0020674169,    and  b2   =   -2.841506. 
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The  reference  path  equation  (2.1)  is  written  as 


yri=y. 


rl    J r2> 


(3.2a) 


jV^^r2^"7^* 


(3.2b) 


where  yrl  is  the  position  of  the  reference  path,  and  yr2  its 
slope.  The  state  equations  (3.1)  are  then  augmented  with  the 
reference  equations  (3.2)  and  the  resulting  system  is  written 
as 


x=Ax+Bb   , 


(3.3) 


where  the  state  vector  x  is 


and  the  A,  B  matrices 


x  = 


v 

i 

y 


[A]    = 


0 

0 

1 

0 

0 

0 

0 

3ll 

312 

0 

0 

0 

0 

321 

a22 

0 

0 

0 

1 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0  0   0   0  -coj  -2Cwn 


15 


[B]     = 


0 

bi 

b2 
0 
0 
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C.  LINEAR  QUADRATIC  REGULATOR  DESIGN 

The  LQR  method  for  determining  controller  gains  involves 
minimizing  a  cost  function  in  which  the  integrand  is  a 
weighted  quadratic  function  of  the  state  x  and  the  control  6, 
such  as 


j=±f(eTQe+STRS)dt  , 


where  T  denotes  transpose  and  e  is  the  error  in  the  state  of 
the  system;  i.e.,  the  difference  between  actual  and  reference 
values.  The  matrices  Q,  R  represent  the  different  weights  in 
the  minimization  index  J.  Large  values  of  R  compared  to  Q 
result  in  significant  penalization  of  the  control  effort  and 
a  soft  control  system.  On  the  other  hand,  if  R  is  relatively 
small,  the  errors  in  the  states  are  penalized  more  and  the 
result  is  increased  rudder  activity  and  a  tight  control 
system. 

In  our  case  we  want  to  minimize  the  errors  in  the  lateral 
deviation  and  the  heading  between  actual  and  desired  vehicles 
path.   Therefore,  we  use 
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J=-|/[(y-yri)2+(i|r-yr2)2+i?62]dt 


or 


J=\f{y2+y2r,-2yyr^2+y2r2-2^yl2+Rb2)  dt  . 

By  looking  at  this  equation  the  appropriate  Q  is  chosen  based 
on  the  state  equations  and  is  of  the  form 
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In  order  to  obtain  the  control  gains  we  must  solve  the 
Algebraic  Riccatti  Equation (ARE)  for  the  positive  matrix  P 

A  TP+PA-PBR-lB  TP+Q=Q 
and  the  optimal  closed  loop  control  is  then 

u=-kx^-R~1BTPx. 

The  gains  k  depend  on  the  selection  of  R.  Figure  3.1  shows  a 
plot  of  the  six  gains  in  k  versus  R,  for  the  values  of  R 
ranging  from  0.01  to  0.5.  It  can  be  seen  that  the  gains 
become  excessively  large  for  R  close  to  0.01,  while  they  get 
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very  small  values  for  R  close  to  0.5.  A  good  compromise  seems 
to  be  about  R  =  0.1,  and  this  value  is  therefore  selected  for 
the  numerical  simulations  that  follow. 

With  all  of  the  preliminary  control  work  complete,  we  can 
now  apply  our  control  gains  to  the  following  control  law 

and  for  our  system  of  equations  the  control  law  takes  the  form 

b=kt{^-a)  +k2v+k2r+k^y+k5yr^k6yr2 

We  are  now  ready  to  analyze  our  system. 
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Figure  3 . 1  k  vs  R  for  Values  0.01  to  0.5 
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IV  SIMULATION  RESULTS 

A.  INTRODUCTION 

Now  that  the  control  system  has  been  developed,  we  must 
fine  tune  it  in  order  to  insure  that  the  system  works 
properly.  A  Matlab  program  was  written  which  first  uses  a  LQR 
minimization  to  solve  the  Algebraic  Riccatti  Equation  for  the 
controller  gains  and  then  uses  an  Euler  integration  to 
simulate  the  response  of  the  control  system.  The  maneuvering 
of  any  marine  vehicle  depends  greatly  on  the  advance  and 
transfer  characteristics  of  the  vessel.  Therefore,  the 
effectiveness  of  our  system  depends  largely  on  what  distance 
from  the  end  of  a  designated  path  that  the  vehicle  must  begin 
to  turn  in  order  to  go  to  the  next  designated  path.  This 
distance  is  designated  as  the  target  distance.  First 
different  target  distances  are  looked  at  for  one  angle  a. 
Then  the  chosen  target  distance  is  applied  to  a  variety  of 
angles  to  ensure  conformity. 

B.  TARGET  DISTANCE  SELECTION 

By  using  a  =  45°,  target  distances  ranging  from  1.0  to  4.0 
shiplengths  were  used  in  increments  of  0.5.  For  each  target 
distance  three  separate  qualifiers  were  looked  at  in  order  to 
determine  the  effectiveness  of  the  system.   These  were  the 
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global  path,  which  showed  that  the  system  was  traveling  the 
desired  smooth  path,  the  local  path,  which  showed  how  the 
vehicle  was  following  the  reference  path,  and  the  rudder 
angle,  which  gave  indications  of  how  hard  the  mechanical 
system  had  to  work. 

For  an  a  =  45°  and  a  target  distance  =  1.0  we  found  that 
the  system  would  overshoot  the  path  and  had  difficulty 
following  the  reference  path  (figures  4.1  and  4.2).  In 
addition,  figure  4.3  shows  that  the  mechanical  system  would 
have  to  work  very  hard  to  try  and  keep  the  vehicle  on  track. 
Figures  4.4  and  4.7  show  that  for  an  a  =  45°  and  target 
distances  of  1.5  and  2.0  the  system  would  produce  a  smooth 
path  that  that  was  smooth  in  nature  and  close  to  the  desired 
path.  However,  the  local  path  showed  that  the  actual  track 
was  not  following  the  reference  path  closely  (figures  4.5  and 
4.8).  Figures  4.6  and  4.9  show  the  rudder  movement  of  the 
system,  indicating  that  the  system  must  still  work  hard  to 
maintain  the  path. 

As  we  moved  to  larger  values  of  target  distance,  the 
actual  path  and  the  reference  path  moved  closer  to  each  other. 
Local  paths  could  be  generated  that  where  almost  on  top  of 
each  other.  This  basically  held  true  for  all  of  the  values 
beyond  the  2.5  shiplength  target  distance,  however,  for  the 
3.5  and  4.0  target  distance,  it  was  found  that  the  global  path 
would  stray  from  the  original  path  (figures  4.10  thru  4.15). 
This  is  attributed  to  the  fact  that  the  reference  path  is 
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being  generated  too  far  away  from  the  new  track  and  too  much 
of  the  second  order  reference  path  was  generated.  After 
carefully  looking  at  these  graphs,  it  seemed  that  the  best 
choice  for  a  target  distance  would  be  the  2.5  shiplengths 
(figures  4.16  thru  4.18).  This  distance  seemed  to  provide  the 
optimum  performance  of  the  control  system  by  producing  the 
desired  smooth  path,  with  accurate  path  following,  and  not 
overworking  of  the  mechanical  system. 

C.   APPLICATION 

Now  that  a  target  of  2.5  has  been  chosen,  all  angles  in 
the  quadrant  were  observed  to  ensure  that  the  selected  target 
distance  would  perform  for  all  angles  (figures  4.19  thru 
4.42).  It  was  found  that  the  2.5  shiplength  target  distance 
would  work  satisfactorily  for  all  of  the  angles  in  the 
quadrant.  The  desired  smooth  path  was  produced  while  the 
track  followed  closely  to  the  reference  path.  It  seems  that 
the  only  possible  draw  back  to  using  the  constant  target 
distance  is  the  work  of  the  mechanical  system.  As  we  move  to 
the  higher  angles,  the  system  must  max  out  the  rudder  for 
longer  periods  of  time.  In  comparison  this  is  a  relatively 
short  period  of  time  and  should  not  pose  a  problem. 
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Figure  4.1  Global  Path  for  a   =  45°  and  Target  Distance  =1.0 
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Figure  4.2  Local  Path  for  a   =  45°  and  Target  Distance  =  1.0 
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Figure   4.3   Rudder  Angle   for  a  =   45°    and  Target   Distance=1.0 
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Figure  4.4  Global  Path  for  a  =  45°  and  Target  Distance  =  1.5 
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Figure  4.5  Local  Path  for  a  =  45°  and  Target  Distance  =  1.5 
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Figure  4.6  Rudder  Angle  for  a  =  45°  and  Target  Distance  =1.5 
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Figure  4.7  Global  Path  for  a  =  45°  and  Target  Distance  =2.0 
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Figure  4.8  Local  Path  for  a   =  45°  and  Target  Distance  =  2.0 
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Figure  4.9  Rudder  Angle  for  a  =  45°  and  Target  Distance  =2.0 
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Figure  4.10  Global  Path  for  a  =  45°  and  Target  Distance  =3.5 
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Figure  4.11  Local  Path  for  a  =  45°  and  Target  Distance  =  3.5 
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Figure  4.12  Rudder  Angle  for  a  =  45°  and  Target  Distance=3.5 
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Figure  4.13  Global  Path  for  a  =  45°  and  Target  Distance  =4.0 
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Figure  4.14  Local  Path  for  a  =  45°  and  Target  Distance  =4.0 
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Figure  4.15  Rudder  Angle  for  a  =  45°  and  Target  Distance=4.0 
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Figure  4.16  Global  Path  for  a  =  45°  and  Target  Distance  =2.5 
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Figure  4.17  Local  Path  for  a  =  45°  and  Target  Distance  =2.5 
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Figure   4.18   Rudder  Angle    for  a   =   45°    and  Target   Distance=2.5 
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Figure  4.19  Global  Path  for  a  =  10' 


Figure  4.20  Local  Path  for  a  =  10' 
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Figure  4.21  Rudder  angle  for  a   =  10 
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Figure  4.22  Global  Path  for  a  =  20° 


u 


Figure  4.23  Local  Path  for  a  =  20° 
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Figure   4.24    Rudder  Angle   for  a  ■   20' 
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Figure  2.25  Global  Path  for  a   =  30 
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Figure    4.26    Local    Path    for   a   =    30' 
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Figure  4.27  Rudder  Angle  for  a  =  30° 
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Figure  4.28  Global  Path  for  a  =  40° 
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Figure    4.29   Local   Path    for   a   =   40 
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Figure  4.30  Rudder  Angle  for  a   =  40 
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Figure  4.31  Global  Path  for  a  =50° 
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Figure  4.32  Local  Path  for  a  =  50° 
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Figure  4.33  Rudder  Angle  for  a   =  50 
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Figure  4.34  Global  Path  for  a  =  60 


2 

> 

c 

\ 

c 

1.5 

- 

\ 

re 

\ 

5 

\ 

u 

\ 

■a 

\ 

— 

A 

re 
C 

1 

\                                             '                                                                          1 

s 

0.5 
n 

0  2  4  6  8  10  12  14  16  18 

time 


Figure   4.35    Local   Path    for  a   =   60 
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Figure  4.36  Rudder  Angle  for  a   =  60 
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Figure  4.37  Global  Path  for  a  =  70 
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Figure  4.38  Local  Path  for  a  =  70 


47 


Figure  4.39  Rudder  Angle  for  a  =  70' 
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Figure  4.40  Global  Path  for  a   =  80' 


Figure  4.41  Local  Path  for  a  =  80° 
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Figure   4.42   Rudder  Angle   for  a  =   80 
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CONCLUSIONS  AND  RECOMMENDATIONS 

A.  CONCLUSIONS 

A  method  for  developing  a  smooth  reference  path  has 
been  developed  and  presented.  This  technique  is  suitable  to 
any  type  of  marine  vehicle  and  not  limited  to  the  test  subject 
in  this  thesis.  It  has  been  shown  that  second  order  models 
are  very  effective  in  providing  a  smooth  path  between  two 
arbitrary  straight  line  segments.  The  LQR  technique  used  is 
very  effective  in  designing  a  control  law  to  drive  the  vehicle 
to  the  reference  path.  Finally  the  turn  initiation  and 
reference  path  generation  must  conform  to  vehicle  advance  and 
transfer  characteristics. 

B .  RECOMMEND AT I ONS 

Some  recommendations  for  further  research  are  as  follows: 

-  Study  of  the  effects  of  sensor  noise  and  imperfection. 

-  The  comparison  with  other  control/guidance  schemes. 
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APPENDIX  A 

% 

%  ship  path  keeping  control  problem  (waypoint) 

% 

%  vehicle  coefficients 

% 

all  =-1.526394; 
al2  =-0.5096502; 
a21  =  2.8750753E-02; 
a22  =-1.449505; 
bl   =  2.0674169E-03; 
b2   =-2.841506; 
% 

%  simulation  time  and  time  step 
% 

stime=500; 
deltat=0.1; 
itime=stime/deltat ; 
% 

%  damping  ratio  and  natural  frequency  for  the  reference 
path       % 
zeta=l; 
wn=l; 
% 

%  matrices  for  state  space  control  design 
% 

0  0 
0  0 
0  0 
0  0 
0       1 

-wn"2   -2*zeta*wn]; 
b=[0;bl;b2;0;0;0]; 
% 

%  weighing  matrices  for  LQR  minimization 
% 


a=[0 

0 

1 

0 

0 

all 

al2 

0 

0 

a21 

a22 

0 

1 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

q=[l 

0 

0 

0 

0 

-lj 

1     •    •    • 

0 

0 

0 

0 

0 

o, 

•    •    • 

0 

0 

0 

0 

0 

0; 

1     *     •     * 

0 

0 

0 

1 

-1 

0| 

1     •    •    • 

0 

0 

0 

-1 

1 

o, 

■    •    •    • 

-1 

0 

0 

0 

0 

1 

I; 

r=0.1; 
% 

%  solve  riccatti  equation;  compute  gains 
% 

[k,s]=lqr(a,b,q,r) ; 
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% 
% 
% 

% 


kl=k(l) 
k2=k(2) 
k3=k(3) 
k4=k(4) 
k5=k(5) 
k6=k(6) 


define  waypoints  and  target  distance 

ipts=3; 

target=input ( 'target=  ' ) ; 

ta=input( 'angle=   ' ) ; 

xdes=[0  5  5+15*cos(ta*pi/180) ]; 

ydes=[0  0  15*sin( ta*pi/180 ) ] ; 

xl=xdes ( 1 ) ; 

yl=ydes ( 1 ) ; 

x2=xdes ( 2 ) ; 

y2=ydes (2 ) 


alpha=atan( (y2-yl ) / (x2-xl ) )  ; 
xtotal=sqrt ( (xl-x2 ) A2+ (yl-y2 ) 

initial  conditions 


2) 


psi(l)=0; 

v(l)=0 

r(l)=0 

y(l)=0 

x(l)=0 

dr(l)=0; 

yp ( 1 )  =  ( Y ( 1 ) -y 1 ) *cos ( alpha ) - ( x ( 1 ) -xl ) *  sin ( alpha ) 

yr 1 ( 1 ) =y ( 1 ) *cos ( alpha )  ; 

yr2 ( 1 ) =-tan ( alpha ) ; 

xr(l)=x(l); 

yr(l)=y(l); 

xawayf=0; 

timef=0; 

xinit=x( 1 ) ; 

yinit=y(l) ; 

time(l)=0; 

j=i; 

istart=2 ; 

start  simulation 
loop  over  waypoints 

for  ipt=l:ipts-l 
xl=xdes (ipt ) ; 
yl=ydes (ipt) ; 
x2=xdes(ipt+l) ; 
y2=ydes (ipt+1) ; 
xl2=x2-xl; 
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yl2=y2-yl; 

alpha=atan( (yl2)/(xl2) ) ; 
beta=alpha; 
alpha=abs( alpha) ; 
if  xl2  >=  0 
if  yl2  >=0 

alpha  =  alpha; 
end 
if  yl2  <  0 

alpha  =  -alpha; 
end 
end 
if  xl2  <  0 

if  yl2  >=  0 

alpha  =  pi-alpha; 
end 
if  yl2  <  0 

alpha  =  pi+alpha; 
end 
end 

a=(yinit-yl ) *cos (alpha) - (xinit-xl ) *sin( alpha) ; 
if  a  ==  0; 

capt=0; 
else 

capt  =  +beta/ (a-beta) ; 
end; 

yo  =  a/ (exp( -capt ) * ( 1+capt ) ) ; 
xtotal=sqrt( (xl-x2) A2+(yl-y2) "2) ; 
% 

%loop  for  each  waypoint 
% 

for  i=istart :itime 
time ( i ) =i*deltat ; 
t= ( i-istart ) *deltat ; 

j-j+l? 

if  j  ==  10 

sim_time  =  time(i) 
alpha 
j=0; 
end 
% 

%  equations  of  motion 
% 

psidot (i-1 ) 
vdot(i-l) 
rdot(i-l) 
ydot(i-l) 
xdot(i-l) 
% 

%  Euler  integration 
% 


54 


psi(i) 

=  psi(i-l)  +  deltat 

*  psidot(i-! 

D 

v(i) 

=  v(i-l)    +  deltat 

*  vdot(i-l) ] 

r(i) 

=  r(i-l)    +  deltat 

*  rdot(i-l) | 

y(i) 

=  y(i-l)    +  deltat 

*  ydot(i-l) | 

x(i) 

=  x(i-l)    +  deltat 

*  xdot(i-l) ; 

yrl(i) 

=  yo*exp(-(t+capt) ) * 

r( 1+t+capt) ; 

% 

% 
% 

yr2(i) 

=  -(t+capt) *yo*exp(- 

•(t+capt) ) ; 

coordinate 

changes 

yp(i)=(y(i) -yl) *cos( alpha) -(x(i) -xl ) *sin( alpha) ; 
xprime= ( x ( i ) -xl ) *cos (alpha) + (y (i) -yl ) * sin (alpha) 
xaway=xtotal-xprime ; 
ypr=yrl(i) ; 
xpr=xprime; 

yr (i) =yl+xpr*sin( alpha) +ypr*cos (alpha) ; 
xr (i)=xl+xpr*cos (alpha) -ypr* sin (alpha) ; 
% 

%  control  law 
% 

dr(i)  =  -(  kl*(psi(i)-alpha)+. . . 
k2*v(i)+. . 
k3*r(i)+. . 
k4*yp(i)+. 
.  k5*yrl(i)+ 
k6*yr2(i)); 
if  dr(i)  >  0.4 

dr(i)=0.4; 
end 
if  dr(i)  <  -0.4 

dr(i)=-0.4; 
end 
% 

%  hit  test;  turn  indication  point 
% 

xaway=xtotal-xprime ; 
if  xaway  <=  target 
istart=i+l;  xinit=x(i);  yinit=y(i); 
xawayf=xaway;timef=time(i) ; 
break,  end 
% 

end 
end 

dr ( 1 ) =dr ( 2 ) ; 
% 

%  plot  results 
% 

plot(x,y,xr,yr, ' : ' ,xdes,ydes, 'o' ) , xlabel( 'x' ) ,ylabel( 'y' ) , 

title ( 'global  path') 
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print 

plot (time, yp, time, yrl ) ,y label ( 'path  deviation' ) , . . . 

xlabel ( ' time ' ) , title ( ' local  path ' ) 
print 
plot ( time , dr ) , xlabel ( ' time ' ) , ylabel ( ' rudder  angle ' ) 

%  clear 
end 
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